#include "Components/ATK_MS901M/atk_ms901m.h"
#include "Components/algorithm.h"
#include "Components/basic.h"
#include "Components/line_patrol.h"
#include "Components/motion.h"
#include "Components/motor.h"
#include "Components/oled_hardware_i2c.h"
#include "Components/servo.h"
#include "Components/simple_json.h"
#include <ti/driverlib/m0p/dl_interrupt.h>

#include <math.h>

#define TAG "Main"

int main(void) {
  SYSCFG_DL_init();
  SysTick_Init();

  uart_init();
  OLED_Init();
  motion_init();
  OLED_ShowString(0, 0, "micro second Tick Test", 16);

  float left_speed = 0;
  float right_speed = 0;

  uint32_t i = 0;

  while (1) {
#if 1
    left_speed = motor_get_speed(MOTOR_REAR | MOTOR_LEFT);
    right_speed = motor_get_speed(MOTOR_REAR | MOTOR_RIGHT);

    // left_speed = sin(i/100.0f);
    // right_speed = cos(i/100.0f);

    my_printf("<%d>{speed}%f,%f\n", i++, left_speed, right_speed);
#endif
    mspm0_delay_ms(1);
  }
}
